/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "PositionFileWriter.hpp"
CPositionFileWriter::CPositionFileWriter()
  :iFile(NULL)
{



}
//*****************************************************************************
CPositionFileWriter::~CPositionFileWriter()
{
  CloseFile();


}
//*****************************************************************************

void CPositionFileWriter::OpenFile(std::string aFileName,
                                   std::string aMaCIGroup,
                                   std::string aInstanceName)
{
 
  
  iFile = fopen(aFileName.c_str(),"wt");
  //Version 2.0 writes all in 3D
  fprintf(iFile,"%%MaCI_Position log file 2.0\n");
  fprintf(iFile,"%%Group: %s Instance: %s\n",aMaCIGroup.c_str(),aInstanceName.c_str());
  
  fprintf(iFile,"%%");
  
      
  fprintf(iFile,"timestamp_sec timestamp_usec ");
 
       
  fprintf(iFile,"position_x position_y position_z angle_x angle_y angle_z ");
  

  fprintf(iFile,"variance_x variance_y variance_z ");
  fprintf(iFile,"variance_angleX variance_angleY variance_angleZ ");
  
       
  fprintf(iFile,"probability ");
  
  fprintf(iFile,"speed angularSpeed");

  fprintf(iFile,"\n\n");

}
//*****************************************************************************

void CPositionFileWriter::CloseFile()
{
  if(iFile){
    fclose(iFile);
    
    iFile = NULL;
  }
}
//*****************************************************************************

bool CPositionFileWriter::WriteLine(TPositionLogLine aLine)
{
 
  if(iFile){
    return WriteLine(TPosition3DLogLine(aLine.timestamp_s,
                                        aLine.timestamp_us,
                                        aLine.positionX,
                                        aLine.positionY,
                                        0.0,
                                        0.0,
                                        0.0,
                                        aLine.heading,
                                        aLine.varianceX,
                                        aLine.varianceY,
                                        0.0,
                                        0.0,
                                        0.0,
                                        aLine.varianceHeading,
                                        aLine.probability,
                                        aLine.speed,
                                        aLine.angularSpeed));

  }else{
    dPrint(1,"FILE NOT OPENED. Could not write a line");
    return false;
  }


}
//*****************************************************************************

bool CPositionFileWriter::WriteLine(TPosition3DLogLine aLine)
{
 if(iFile){
    fprintf(iFile,"%u %u %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f",
            aLine.timestamp_s,
            aLine.timestamp_us,
            aLine.positionX,
            aLine.positionY,
            aLine.positionZ,
            aLine.angleX,
            aLine.angleY,  
            aLine.angleZ,  
            aLine.varianceX,
            aLine.varianceY,
            aLine.varianceZ,
            aLine.varianceAngleX,
            aLine.varianceAngleY,
            aLine.varianceAngleZ,
            aLine.probability,
            aLine.speed,
            aLine.angularSpeed);

    fprintf(iFile,"\n");

    fflush(iFile);
    return true;
  }else{
    dPrint(1,"FILE NOT OPENED. Could not write a line");
    return false;
  }

}
//*****************************************************************************

bool CPositionFileWriter::WriteLine(MaCI::Position::CPositionData &aData)
{
  using namespace MaCI::Position;
 
  if(iFile){
    const MaCI::Common::TTimestamp *tstamp = aData.GetTimestamp();
   
    if(tstamp){
      fprintf(iFile,"%u %u ",tstamp->timestamp_s,tstamp->timestamp_us);
    }else{
      fprintf(iFile,"0 0 ");
    }
    
        
    const TPose3D *pos3D = aData.GetPose3D();
    if(pos3D){
      fprintf(iFile,"%f %f %f %f %f %f ",pos3D->x,pos3D->y,pos3D->z,pos3D->eul_x,pos3D->eul_y,pos3D->eul_z);

    }else{
      const TPose2D *pos = aData.GetPose2D();
      if(pos){

        fprintf(iFile,"%f %f 0.0 0.0 0.0 %f ",pos->x,pos->y,pos->a);
      }else{
        fprintf(iFile,"0.0 0.0 0.0 0.0 0.0 0.0 ");
      }
    }
    
    
    const TVariance3D *var3D = aData.GetVariance3D();
    if(var3D){
 
       fprintf(iFile,"%f %f %f %f %f %f ",var3D->x,var3D->y,var3D->z,var3D->eul_x,var3D->eul_y,var3D->eul_z);
    }else{
      const TVariance2D *var = aData.GetVariance2D();
      if(var){
 
        fprintf(iFile,"%f %f 0.0 0.0 0.0 %f ",var->x,var->y,var->a);
      }else{
 
        fprintf(iFile,"0.0 0.0 0.0 0.0 0.0 0.0 ");
      }
    }
    
    
    
    const TProbability *prob = aData.GetProbability();
    if(prob){
      fprintf(iFile,"%f ",prob->probability);
    }else{
      fprintf(iFile,"0.0 ");
    }
    
    
    
    const TSpeed2D *speed = aData.GetSpeed2D();
    if(speed){
      fprintf(iFile,"%f %f ",speed->speed, speed->angularSpeed);
    }else{
      fprintf(iFile,"0.0 0.0 ");
    }
    
    fprintf(iFile,"\n");

    fflush(iFile);

    return true;
  }else{
    dPrint(1,"FILE NOT OPENED. Could not write a line");
    return false;
  }


}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
